(* insertion heuristic *)

open Uav_data

(* returns cost of insertion of task t at position i of plan p of aircraft k *)
let insertion_cost = fun k t p i ->
  time.(k).(p.(i-1)).(t) + time.(k).(t).(p.(i)) - time.(k).(p.(i-1)).(p.(i))

(* [insert_at_position t p i] inserts the task [t] at the position [i] 
 * of plan p *)
let insert_at_position = fun t p i ->
  let rec insertion = fun t p i ->
    if i = 0
    then t :: p
    else List.hd p :: insertion t (List.tl p) (i-1)
  in Array.of_list (insertion t (Array.to_list p) i)

(* insert the task t in the plan p of ac_id *)
let insert_at_best_position = fun t p ac_id ->
  let lower_cost = ref max_int
  and position = ref (-1) in

    for i=1 to Array.length p -1 do
      let ic = insertion_cost ac_id t p i in
      if ic < !lower_cost then
        begin
          lower_cost := ic;
	  position := i
	end
    done;
    insert_at_position t p !position

(* insertion cost the task t in the plan p of ac_id *)
let insertion_cost_at_best_position = fun t p ac_id ->
  let lower_cost = ref max_int
  and position = ref (-1) in

    for i=1 to Array.length p -1 do
      let ic = insertion_cost ac_id t p i in
      if ic < !lower_cost then
        begin
          lower_cost := ic;
	  position := i
	end
    done;
    !lower_cost

(* *)
let generate_plans = fun tasks ->
  let plans = Array.init m (fun i -> [|n+i; n+m+i|]) in
  let todo = Array.init m (fun _ -> []) in
    for i=0 to Array.length tasks-1 do
      todo.(tasks.(i)) <- i :: todo.(tasks.(i));
    done;
    for i =0 to m-1 do
      for j=0 to List.length todo.(i) -1 do
	plans.(i) <- insert_at_best_position (List.nth todo.(i) j) plans.(i) i;
      done
    done;
    plans
      
